#include "pid.h"
#include "mid/printf.h"

void DumpPID(PID* pid)
{
    printf("PID| kP=%.2f kI=%.2f kD=%.2f maxI=%.2f maxOutput=%.2f target=%d\n"
           "     dP=%.2f dI=%.2f dD=%.2f error=%.2f lastError=%.2f\n"
           "     output=%.2f\n",
            pid->kP, pid->kI, pid->kD, pid->maxDeltaI, pid->maxOutput, pid->target,
            pid->dP, pid->dI, pid->dD, pid->error, pid->lastError, pid->output
        );
}

void InitPID(PID *pid, float kP, float kI, float kD, float maxDeltaI, float maxOutput, int target, int maxTarget)
{
    pid->kP = kP;
    pid->kI = kI;
    pid->kD = kD;
    pid->maxDeltaI = maxDeltaI;
    pid->maxOutput = maxOutput;
    pid->target = target;
    pid->maxTarget = maxTarget;
    pid->error = .0f;
    pid->lastError = .0f;
    pid->dP = .0f;
    pid->dI = .0f;
    pid->dD = .0f;
    pid->output = 0;
}

float CalculatePID(PID *pid, float target, float current)
{
    pid->lastError = pid->error;
    pid->error = target - current;

    // 计算 dP, dI, dD
    pid->dP = pid->error;
    pid->dI += pid->error;
    pid->dD = pid->error - pid->lastError;

    // checkout dI bound
    if (pid->dI > pid->maxDeltaI)
        pid->dI = pid->maxDeltaI;
    else if (pid->dI < -pid->maxDeltaI)
        pid->dI = -pid->maxDeltaI;

    pid->output = pid->kP * pid->dP + pid->kI * pid->dI + pid->kD * pid->dD;
    // printf("current=%.2f target=%.2f\n", current, target);
    // DumpPID(pid);
    if (pid->output > pid->maxOutput)
        pid->output = pid->maxOutput;
    else if (pid->output < -pid->maxOutput)
        pid->output = -pid->maxOutput;
    // DumpPID(pid);
    return pid->output;
}